#include "BEMUtil.h"

double shiftAlpha = 1e-6;
double poissonRatio = 0;
double shearModulus = 1;
double Pi = 3.1416;

double* getDrI(Vec2d r, double rn, Mat22d I)
{
	double s = log(rn);
	Mat22d A = I;
	Vec2d ds = r / (rn * rn);

	double dA[2][2][2];

	for (int i = 0; i < 2; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			for (int k = 0; k < 2; k++)
			{
				dA[i][j][k] = 0;
			}
		}
	}

	return tensorProductGradient(s, ds, A, dA);
}


double* getDrM(Vec2d r, double rn, Mat22d M)
{
	double down = rn * rn * rn * rn;
	double up1 = r[0] * r[0] - r[1] * r[1];
	double up2 = -1 * up1;

	double s = 1.0;
	Mat22d A = M;
	Vec2d ds(0, 0);

	double dAm[2][4];

	dAm[0][0] = 2 * r[0] * r[1] * r[1] / down;
	dAm[0][1] = r[1] * up2 / down;
	dAm[0][2] = -2 * r[0] * r[0] * r[1] / down;
	dAm[0][3] = r[0] * up1 / down;
	

	dAm[1][0] = r[1] * up2 / down;
	dAm[1][1] = -2 * r[0] * r[1] * r[1] / down;
	dAm[1][2] = r[0] * up1 / down;
	dAm[1][3] = 2 * r[0] * r[0] * r[1] / down;	



	double dA[2][2][2];


	dA[0][0][0] = dAm[0][0];
	dA[0][1][0] = dAm[0][1];
	
	dA[1][0][0] = dAm[1][0];
	dA[1][1][0] = dAm[1][1];
	

	dA[0][0][1] = dAm[0][2];
	dA[0][1][1] = dAm[0][3];
	
	dA[1][0][1] = dAm[1][2];
	dA[1][1][1] = dAm[1][3];
	

	return tensorProductGradient(s, ds, A, dA);
}


double* tensorProductGradient(double s, Vec2d ds, Mat22d A, double dA[2][2][2])
{
	double K[2][4];

	for (int i = 0; i<2; i++)
	{
		for (int j = 0; j<2; j++)
		{
			for (int k = 0; k<2; k++)
			{
				K[j][k + i * 2] = A(j, k) * ds[i];
			}
		}
	}

	double dsA[2][2][2];
	double* M = (double*)malloc(8 * sizeof(double));

	dsA[0][0][0] = K[0][0];
	dsA[0][1][0] = K[1][0];
	
	dsA[1][0][0] = K[0][1];
	dsA[1][1][0] = K[1][1];	

	dsA[0][0][1] = K[0][2];
	dsA[0][1][1] = K[1][2];
	
	dsA[1][0][1] = K[0][3];
	dsA[1][1][1] = K[1][3];
	

	for (int i = 0; i<2; i++)
	{
		for (int j = 0; j<2; j++)
		{
			for (int k = 0; k<2; k++)
			{
				M[getIndex(i, j, k)] = dsA[i][j][k] + s * dA[i][j][k];
			}
		}
	}

	return M;
}


int getIndex(int i, int j, int k)
{
	return i * 4 + j * 2 + k;
}


double* getKernelGradient(Vec2d p, Vec2d q)
{
	Vec2d r = p - q;

	double rn = mag(r);

	if (rn < shiftAlpha)
	{
		rn = shiftAlpha;
	}

	Vec2d rg = r / rn;

	double Me[4];
	double Ie[4];

	Me[0] = rg[0] * rg[0];
	Me[1] = rg[0] * rg[1];
	Me[2] = rg[1] * rg[0];
	Me[3] = rg[1] * rg[1];	
	

	Ie[0] = 1;
	Ie[1] = 0;
	Ie[2] = 0;
	Ie[3] = 1;
	

	Mat22d M(Me);
	Mat22d I(Ie);

	double* DrM = getDrM(r, rn, M);
	double* DrI = getDrI(r, rn, I);
		

	double* Z = (double*)malloc(8 * sizeof(double));

	for (int i = 0; i<2; i++)
	{
		for (int j = 0; j<2; j++)
		{
			for (int k = 0; k<2; k++)
			{
				Z[getIndex(i, j, k)] = -1.0 * ((3.0 - 4.0 * poissonRatio) * DrI[getIndex(i, j, k)] - DrM[getIndex(i, j, k)]) / (8.0 * Pi * shearModulus * (1.0 - poissonRatio));
			}
		}
	}

	free(DrM);
	free(DrI);

	return Z;
}


Mat22d tensorVectorDot(double A[2][2][2], Vec2d v)
{
	double B[4][2];

	B[0][0] = A[0][0][0];
	B[1][0] = A[1][0][0];
	B[2][0] = A[0][1][0];
	B[3][0] = A[1][1][0];
	

	B[0][1] = A[0][0][1];
	B[1][1] = A[1][0][1];
	B[2][1] = A[0][1][1];
	B[3][1] = A[1][1][1];	

	double C[4];

	for (int i = 0; i<4; i++)
	{
		C[i] = 0;

		for (int j = 0; j<2; j++)
		{
			C[i] += B[i][j] * v[j];
		}
	}

	Mat22d MT(C);

	return MT;
}


Mat22d tensorVectorContraction(double A[2][2][2], Vec2d v)
{
	double B[2][4];

	B[0][0] = A[0][0][0];
	B[0][1] = A[1][0][0];
	B[0][2] = A[0][0][1];
	B[0][3] = A[1][0][1];
	
	B[1][0] = A[0][1][0];
	B[1][1] = A[1][1][0];
	B[1][2] = A[0][1][1];
	B[1][3] = A[1][1][1];
	

	double C[4];

	for (int i = 0; i<4; i++)
	{
		C[i] = 0;

		for (int j = 0; j<2; j++)
		{
			C[i] += B[j][i] * v[j];
		}
	}

	Mat22d MT(C);

	return MT;
}


double tensorTensorContraction(Mat22d A, Mat22d B)
{
	double c = 0;

	for (int i = 0; i < 2; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			c += A(i, j) * B(i, j);
		}
	}

	return c;
}


Mat22d evaluateGradientQuadraturePoint(Vec2d p, Vec2d q, Vec2d qn)
{
	double* Kd = getKernelGradient(p, q);

	double K[2][2][2];

	for (int i = 0; i<2; i++)
	{
		for (int j = 0; j<2; j++)
		{
			for (int k = 0; k<2; k++)
			{
				K[i][j][k] = Kd[getIndex(i, j, k)];
			}
		}
	}	

	free(Kd);

	return tensorVectorDot(K, qn);
}


Mat22d evaluateQuadraturePoint(Vec2d p, Vec2d q)
{
	Vec2d r = p - q;

	double rn = mag(r);

	if (rn < shiftAlpha)
	{
		rn = shiftAlpha;
	}

	Vec2d rg = r / rn;

	double Me[4];
	double Ie[9];

	Me[0] = rg[0] * rg[0];
	Me[1] = rg[0] * rg[1];
	Me[2] = rg[1] * rg[0];
	Me[3] = rg[1] * rg[1];
	

	Ie[0] = 1;
	Ie[1] = 0;
	Ie[2] = 0;
	Ie[3] = 1;
	

	Mat22d M(Me);
	Mat22d I(Ie);

	Mat22d S;

	S = -1.0 / (8.0 * Pi * (1.0 - poissonRatio) * shearModulus) * ((3 - 4 * poissonRatio) * log(rn + shiftAlpha) * I - M);

	return S;
}


int getTensorArrayIndex(int i, int j, int k)
{
	return k * 4 + i * 2 + j;
}